#ifndef CTRLARCHITECTURE_H
#define CTRLARCHITECTURE_H

#include <Eigen/Dense>
#include "IIRfilter.hpp"
#include "RbtCtrller.hpp"


/*
different controller architecture integrate here, 

currently support controlled object:    1. single motor
                                        2. scara robot

currently support control method: 1. double loop PID
                                  2. double loop Loop shaping
                                  3. ......
*/
namespace ctrl{

/*single motor control************************************************************************************************************/
void DoubleLoopCtrl(double pos_ref, double vel_ref, double pos_fb, double vel_fb, IIR_filt pos_ctrller, IIR_filt vel_ctrller, double FFCout, double doubleloop_out){
    double pos_ctrller_out, vel_ctrller_out;
    pos_ctrller_out = pos_ctrller(pos_ref-pos_fb);
    vel_ctrller_out = vel_ctrller(vel_ref + pos_ctrller_out - vel_fb);
    doubleloop_out = vel_ctrller_out + FFCout;
}
/*single motor control************************************************************************************************************/




/*robot control*****************************************************************************************************************/
template<class T>
void RbtDoubleLoopCtrl(T *robot, rbt_ctrller *rbt_ctrller){
    for (int id = 0; id < robot->NumAxis(); id++){
        dynamica::Axis *axis = & robot->axes_[id];
        IIR_filt pos_controller = rbt_ctrller->doubleloop_pc[id];
        IIR_filt vel_controller = rbt_ctrller->doubleloop_vc[id];
        DoubleLoopCtrl(axis->qcmd, axis->qdcmd, axis->qfb, axis->qdfb, pos_controller, vel_controller, 0, axis->taucmd);
    }
}
/*robot control*****************************************************************************************************************/
}
#endif